/**
 * @file ebex_quaternion.h
 *
 * @date Jan 1, 2010
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef EBEX_QUATERNION_H_
#define EBEX_QUATERNION_H_

#include <stdbool.h>
#include <float.h>
#include <math.h>

#include <ebex_math.h>
#include <ebex_vector.h>
#include <ebex_lookup.h>
#include <random_util.h>

#define _ebex_quat_filter_types(x,_)	\
	_(x, none)							\
	_(x, map)							\
	_(x, mmse)							\
	_(x, weighted_mean)

EBEX_LOOKUP_TABLE(ebex_quat_filter_type, static);

void ebex_matrix_from_quat( ebex_mat3x3_t *m_result, const ebex_quat_t *m_src );
void ebex_quat_from_matrix( ebex_quat_t *m_result, const ebex_mat3x3_t *m_src );
void ebex_quat_spherical_weighted_mean(ebex_quat_t *m_dest, const ebex_quat_t *m_input, const double *m_weights,
		double m_errtol, int m_maxiter, size_t m_num);
void ebex_quat_spherical_weighted_mean_corrected(ebex_quat_t *m_dest, const ebex_quat_t *m_corrected_quaternions, const double *m_weights,
		double m_errtol, int m_maxiter, size_t m_num);
void ebex_quat_regularize(const ebex_quat_t *m_input, ebex_quat_t *m_output, size_t m_num, const double m_bandwidth,
		const random_sample_3d_t *m_kernel, const ebex_mat3x3_t *m_intensity );

void ebex_quat_slerp(ebex_quat_t *m_result, double m_percent, const ebex_quat_t *m_unitquat0, const ebex_quat_t *m_unitquat1);

void ebex_quat_propagate(ebex_quat_t *m_result, ebex_quat_t *m_inital, ebex_3vec_t *m_omega, double m_sec);
void ebex_quat_from_euler(ebex_quat_t *m_quat, ebex_3vec_t *m_euler, e_rotation_order m_order);
void ebex_quat_mmse(ebex_quat_t *m_result, const ebex_quat_t *m_input, const double *m_weight, size_t m_num);
void ebex_quat_exp(ebex_quat_t *m_dest, ebex_quat_t *m_exp);
void ebex_quat_log(ebex_quat_t *m_dest, ebex_quat_t *m_logquat);
void ebex_quat_randomize_population(ebex_quat_t *m_quat, random_sample_t *m_rnd, size_t m_num);
void ebex_quat_generate_attitude_cov(ebex_mat3x3_t *m_output, ebex_quat_t *m_input, ebex_quat_t *m_mean, double *m_weights, size_t m_num);
void ebex_quat_smooth(ebex_quat_t *m_dest, size_t m_dest_step_size, ebex_quat_t *m_src, size_t m_src_step_size,
                      double *m_window, size_t m_num, int m_windowsize, size_t m_iterations, double m_gain);

static inline double ebex_quat_get_max_abs(const ebex_quat_t *m_quat)
{
	return MAX(MAX(fabs(m_quat->x), fabs(m_quat->y)), MAX(fabs(m_quat->z), fabs(m_quat->w)));
}

static inline void ebex_quat_copy(ebex_quat_t *m_result, const ebex_quat_t *quat)
{
	m_result->x = quat->x;
	m_result->y = quat->y;
	m_result->z = quat->z;
	m_result->w = quat->w;
}

static inline void ebex_quat_from_elems(ebex_quat_t *m_result, double _x, double _y, double _z, double _w)
{
	m_result->x = _x;
	m_result->y = _y;
	m_result->z = _z;
	m_result->w = _w;
}

static inline void ebex_quat_from_3vec(ebex_quat_t *m_result, const ebex_3vec_t *xyz, double _w)
{
	m_result->x = xyz->x;
	m_result->y = xyz->y;
	m_result->z = xyz->z;
	m_result->w = _w;

}

static inline void ebex_quat_from_scalar(ebex_quat_t *m_result, double m_scalar)
{
	m_result->x = m_scalar;
	m_result->y = m_scalar;
	m_result->z = m_scalar;
	m_result->w = m_scalar;
}

static inline void ebex_quat_add(ebex_quat_t *m_result, const ebex_quat_t *m_quat0, const ebex_quat_t *m_quat1)
{
	m_result->x = (m_quat0->x + m_quat1->x );
	m_result->y = (m_quat0->y + m_quat1->y );
	m_result->z = (m_quat0->z + m_quat1->z );
	m_result->w = (m_quat0->w + m_quat1->w );
}

static inline void ebex_quat_sub(ebex_quat_t *m_result, const ebex_quat_t *m_quat0, const ebex_quat_t *m_quat1)
{
	m_result->x = (m_quat0->x - m_quat1->x );
	m_result->y = (m_quat0->y - m_quat1->y );
	m_result->z = (m_quat0->z - m_quat1->z );
	m_result->w = (m_quat0->w - m_quat1->w );
}

static inline void ebex_quat_scalarmul(ebex_quat_t *m_result, const ebex_quat_t *quat, double m_scalar)
{
	m_result->x = (quat->x * m_scalar );
	m_result->y = (quat->y * m_scalar );
	m_result->z = (quat->z * m_scalar );
	m_result->w = (quat->w * m_scalar );
}

static inline void ebex_quat_scalardiv(ebex_quat_t *m_result, const ebex_quat_t *quat, double m_scalar)
{
	m_result->x = (quat->x / m_scalar );
	m_result->y = (quat->y / m_scalar );
	m_result->z = (quat->z / m_scalar );
	m_result->w = (quat->w / m_scalar );
}

static inline void ebex_quat_neg(ebex_quat_t *m_result, const ebex_quat_t *quat)
{
	m_result->x = -quat->x;
	m_result->y = -quat->y;
	m_result->z = -quat->z;
	m_result->w = -quat->w;
}

static inline double ebex_quat_dot(const ebex_quat_t *m_quat0, const ebex_quat_t *m_quat1)
{
	double m_result;
	m_result = (m_quat0->x * m_quat1->x );
	m_result = (m_result + (m_quat0->y * m_quat1->y) );
	m_result = (m_result + (m_quat0->z * m_quat1->z) );
	m_result = (m_result + (m_quat0->w * m_quat1->w) );
	return m_result;
}

static inline double ebex_quat_norm(const ebex_quat_t *quat)
{
	return (quat->x * quat->x ) + (quat->y * quat->y)  + (quat->z * quat->z) + (quat->w * quat->w);
}

static inline double ebex_quat_length(const ebex_quat_t *quat)
{
	return sqrt(ebex_quat_norm(quat));
}

static inline void ebex_quat_maintain(ebex_quat_t *m_quat)
{
	double norm = ebex_quat_norm(m_quat);
	ebex_quat_scalarmul(m_quat, m_quat, (1.5 - 0.5 * norm));
}

static inline void ebex_quat_normalize(ebex_quat_t *m_result, const ebex_quat_t *quat)
{
	double norm = ebex_quat_norm(quat);

	if (isnormal(norm - 1.0))
	{
		double len_inv = 1.0 / sqrt(norm);
		m_result->x = (quat->x * len_inv );
		m_result->y = (quat->y * len_inv );
		m_result->z = (quat->z * len_inv );
		m_result->w = (quat->w * len_inv );
	}
	else
	{
		if (m_result != quat) ebex_quat_copy(m_result, quat);
	}
}

static inline void ebex_quat_get_xyz(ebex_3vec_t *m_result, const ebex_quat_t *m_src)
{
	double invsintheta = 0.0;

	invsintheta = 1.0 / sin(acos(m_src->w));

	if (!isfinite(invsintheta)) invsintheta = 0.0;

	ebex_3vec_make_from_elems(m_result, m_src->x, m_src->y, m_src->z);
	ebex_3vec_scalar_mul(m_result, m_result, invsintheta);
}




static inline double ebex_quat_get_yaw(ebex_quat_t *m_src)
{
	ebex_3vec_t retval;
	ebex_3vec_euler_from_quat(&retval, rotation_order_XYZ, m_src);
	return retval.yaw;
}

static inline double ebex_quat_get_pitch(ebex_quat_t *m_src)
{
	ebex_3vec_t retval;
	ebex_3vec_euler_from_quat(&retval, rotation_order_XYZ, m_src);
	return retval.pitch;
}

static inline double ebex_quat_get_roll(ebex_quat_t *m_src)
{
	ebex_3vec_t retval;
	ebex_3vec_euler_from_quat(&retval, rotation_order_XYZ, m_src);
	return retval.roll;
}

static inline void ebex_quat_set_identity(ebex_quat_t *m_result)
{
	ebex_quat_from_elems(m_result, 0.0, 0.0, 0.0, 1.0);
}

static inline void ebex_quat_lerp(ebex_quat_t *m_result, double t, const ebex_quat_t *m_quat0, const ebex_quat_t *m_quat1)
{
	ebex_quat_t tmpq_0, tmpq_1;
	ebex_quat_sub(&tmpq_0, m_quat1, m_quat0);
	ebex_quat_scalarmul(&tmpq_1, &tmpq_0, t);
	ebex_quat_add(m_result, m_quat0, &tmpq_1);
}

static inline void ebex_quat_squad(ebex_quat_t *m_result, double m_percent, const ebex_quat_t *m_unitquat0, const ebex_quat_t *m_unitquat1,
		const ebex_quat_t *m_unitquat2, const ebex_quat_t *m_unitquat3)
{
	ebex_quat_t tmp0, tmp1;
	ebex_quat_slerp(&tmp0, m_percent, m_unitquat0, m_unitquat3);
	ebex_quat_slerp(&tmp1, m_percent, m_unitquat1, m_unitquat2);
	ebex_quat_slerp(m_result, ((2.0 * m_percent) * (1.0 - m_percent) ), &tmp0, &tmp1 );
}

static inline void ebex_quat_from_vector_diff(ebex_quat_t *m_result, const ebex_3vec_t *m_e0, const ebex_3vec_t *m_e1)
{
	ebex_3vec_t tmpv0, tmpv1;

	double coshalfangle = (1.0 + ebex_3vec_dot(m_e0, m_e1));
	double tmp = (1.0 / sqrt((2.0 * coshalfangle )) );

	ebex_3vec_cross(&tmpv0, m_e0, m_e1);
	ebex_3vec_scalar_mul(&tmpv1, &tmpv0, tmp);
	ebex_quat_from_3vec(m_result, &tmpv1, coshalfangle);
}

static inline void ebex_quat_from_unit_sphere(ebex_quat_t *m_result, const ebex_3vec_t *m_src)
{
	///Second version.  Pick 0,0,1 as rotation 0,0,0.
	ebex_3vec_t zero_vec;

	ebex_3vec_make_from_elems(&zero_vec, 0.0, 0.0, 1.0);
	ebex_quat_from_vector_diff(m_result, &zero_vec, m_src);
	ebex_quat_normalize(m_result, m_result);
}

static inline void ebex_quat_from_rotation_axis(ebex_quat_t *m_result, double m_radians, const ebex_3vec_t *m_e)
{
	ebex_3vec_t tmpv;
	double s, c;

	m_radians *= 0.5;
	vdSinCos(1, &m_radians, &s, &c);
	ebex_3vec_scalar_mul(&tmpv, m_e, s);
	ebex_quat_from_3vec(m_result, &tmpv, c);
}

static inline void ebex_quat_from_rotation_x(ebex_quat_t *m_result, double m_radians)
{
	double s, c;

	m_radians *= 0.5;
	vdSinCos(1, &m_radians, &s, &c);
	ebex_quat_from_elems(m_result, s, 0.0, 0.0, c);
}

static inline void ebex_quat_from_rotation_y(ebex_quat_t *m_result, double m_radians)
{
	double s, c;

	m_radians *= 0.5;
	vdSinCos(1, &m_radians, &s, &c);
	ebex_quat_from_elems(m_result, 0.0, s, 0.0, c);
}

static inline void ebex_quat_from_rotation_z(ebex_quat_t *m_result, double m_radians)
{
	double s, c;

	m_radians *= 0.5;
	vdSinCos(1, &m_radians, &s, &c);
	ebex_quat_from_elems(m_result, 0.0, 0.0, s, c);
}

static inline void ebex_quat_mul(ebex_quat_t *m_result, const ebex_quat_t *m_orientation, const ebex_quat_t *m_rotation)
{
	double tmpx, tmpy, tmpz, tmpw;
	tmpx = ((((m_orientation->w * m_rotation->x) + (m_orientation->x * m_rotation->w)) + (m_orientation->y * m_rotation->z)) - (m_orientation->z * m_rotation->y) );
	tmpy = ((((m_orientation->w * m_rotation->y) + (m_orientation->y * m_rotation->w)) + (m_orientation->z * m_rotation->x)) - (m_orientation->x * m_rotation->z) );
	tmpz = ((((m_orientation->w * m_rotation->z) + (m_orientation->z * m_rotation->w)) + (m_orientation->x * m_rotation->y)) - (m_orientation->y * m_rotation->x) );
	tmpw = ((((m_orientation->w * m_rotation->w) - (m_orientation->x * m_rotation->x)) - (m_orientation->y * m_rotation->y)) - (m_orientation->z * m_rotation->z) );
	ebex_quat_from_elems(m_result, tmpx, tmpy, tmpz, tmpw);
}

static inline void ebex_quat_rotate_3vec(ebex_3vec_t *m_result, const ebex_quat_t *m_rotation, const ebex_3vec_t *m_vec)
{
	double tmpx = (((m_rotation->w * m_vec->x) + (m_rotation->y * m_vec->z)) - (m_rotation->z * m_vec->y) );
	double tmpy = (((m_rotation->w * m_vec->y) + (m_rotation->z * m_vec->x)) - (m_rotation->x * m_vec->z) );
	double tmpz = (((m_rotation->w * m_vec->z) + (m_rotation->x * m_vec->y)) - (m_rotation->y * m_vec->x) );
	double tmpw = (((m_rotation->x * m_vec->x) + (m_rotation->y * m_vec->y)) + (m_rotation->z * m_vec->z) );

	m_result->x = ((((tmpw * m_rotation->x) + (tmpx * m_rotation->w)) - (tmpy * m_rotation->z)) + (tmpz * m_rotation->y) );
	m_result->y = ((((tmpw * m_rotation->y) + (tmpy * m_rotation->w)) - (tmpz * m_rotation->x)) + (tmpx * m_rotation->z) );
	m_result->z = ((((tmpw * m_rotation->z) + (tmpz * m_rotation->w)) - (tmpx * m_rotation->y)) + (tmpy * m_rotation->x) );
}

static inline void ebex_quat_conj(ebex_quat_t *m_result, const ebex_quat_t *quat)
{
	ebex_quat_from_elems(m_result, -quat->x, -quat->y, -quat->z, quat->w);
}

static inline void ebex_quat_inv(ebex_quat_t *m_result, const ebex_quat_t *m_quat)
{
	ebex_quat_conj(m_result, m_quat);
	ebex_quat_maintain(m_result);
}

static inline void ebex_quat_scale_angle(ebex_quat_t *m_result, const ebex_quat_t *m_quat, const double m_scale)
{
	ebex_quat_t tmp_quat;
	const ebex_quat_t *tmp_quat_p = m_quat;

	double err;

	if (m_quat->w < 0.0)
	{
		/// This keeps the function sane for angles near -pi
		ebex_quat_neg(&tmp_quat, m_quat);
		tmp_quat_p = &tmp_quat;
	}

	/// Watch out for magnitude close to 1.0
	err = tmp_quat_p->w - 1.0;

	/// This _should_ always be true for a normalized quaternion
	if (err < 0.0)
	{

		double ratio;
		double halfangle = acos(tmp_quat_p->w);

		ratio = sin(m_scale * halfangle) / sin(halfangle);

		m_result->x = tmp_quat_p->x * ratio;
		m_result->y = tmp_quat_p->y * ratio;
		m_result->z = tmp_quat_p->z * ratio;
		m_result->w = cos(m_scale * halfangle);

	}
	else if (err < DBL_TINY)
	{
		/// If the quaternion is normalized, this is just a rounding error, so copy
		ebex_quat_copy(m_result, tmp_quat_p);

	}
	else
	{
		die("Bad quaternion!");
	}
	ebex_quat_maintain(m_result);

}

static inline void ebex_quat_print( const ebex_quat_t *quat )
{
    printf( "( %f %f %f %f )\n", quat->x, quat->y, quat->z, quat->w );
}

static inline void ebex_quat_prints( const ebex_quat_t *quat, const char *name )
{
    printf( "%s: ( %f %f %f %f )\n", name, quat->x, quat->y, quat->z, quat->w );
}

static inline bool ebex_quat_isnan( const ebex_quat_t *m_quat )
{
	return isnan(m_quat->x) || isnan(m_quat->y) || isnan(m_quat->z) || isnan(m_quat->w);
}

static inline void ebex_quat_error( ebex_quat_t *m_result, const ebex_quat_t *m_quat1, const ebex_quat_t *m_quat2)
{

	ebex_quat_t result;
	ebex_quat_t quat2;

	if (ebex_quat_dot(m_quat1, m_quat2) < 0.0)
		ebex_quat_neg(&quat2, m_quat2);
	else
		ebex_quat_copy(&quat2, m_quat2);

	result.x = -quat2.w*m_quat1->x - quat2.z*m_quat1->y + quat2.y*m_quat1->z + quat2.x*m_quat1->w;
	result.y =  quat2.z*m_quat1->x - quat2.w*m_quat1->y - quat2.x*m_quat1->z + quat2.y*m_quat1->w;
	result.z = -quat2.y*m_quat1->x + quat2.x*m_quat1->y - quat2.w*m_quat1->z + quat2.z*m_quat1->w;
	result.w =  quat2.x*m_quat1->x + quat2.y*m_quat1->y + quat2.z*m_quat1->z + quat2.w*m_quat1->w;

	ebex_quat_normalize(m_result, &result);
}

static inline void ebex_quat_weighted_sum(ebex_quat_t *m_result, const ebex_quat_t *m_input, const double *m_weights,
		size_t m_num)
{
	ebex_quat_t temp;
	ebex_quat_from_scalar(m_result, 0.0);

	if (m_weights)
	{
		for (size_t i = 0; i < m_num; i++)
		{
			ebex_quat_scalarmul(&temp, &m_input[i], m_weights[i]);
			ebex_quat_add(m_result, m_result, &temp);
		}
	}
	else
	{
		double weight = 1.0 / (double)m_num;
		for (size_t i = 0; i < m_num; i++)
		{
			ebex_quat_scalarmul(&temp, &m_input[i], weight);
			ebex_quat_add(m_result, m_result, &temp);
		}
	}

}

/**
 * This rotates a quaternion by a perpendicular 4-vector.  The length of the 4-vector (m_dir) determines the size of
 * rotation
 * @param m_result Output unit quaternion
 * @param m_input Input unit quaternion
 * @param m_dir Perpendicular, non-unit quaternion
 */
static inline void ebex_quat_rotate_4vec(ebex_quat_t *m_result, const ebex_quat_t *m_input, const ebex_quat_t *m_dir)
{
	double costheta;
	double sintheta;
	double theta = sqrt(ebex_quat_norm(m_dir));
	ebex_quat_t unit_dir;

	if (theta < DBL_EPSILON)
	{
		ebex_quat_copy(m_result, m_input);
		return;
	}

	vdSinCos(1, &theta, &sintheta, &costheta);
	ebex_quat_scalarmul(&unit_dir, m_dir, sintheta/theta);
	ebex_quat_scalarmul(m_result, m_input, costheta);

	ebex_quat_add(m_result, m_result, &unit_dir);

	ebex_quat_normalize(m_result, m_result);
}

static inline double ebex_quat_arg(ebex_quat_t *m_quat)
{
	ebex_3vec_t svec = {{m_quat->x, m_quat->y, m_quat->z}};
	return atan2(ebex_3vec_length(&svec), m_quat->w);
}

/**
 * Raises a quaternion to the power of another quaternion.
 * @param m_dest Pointer to the destination quaternion
 * @param m_base Pointer to the base quaternion
 * @param m_pow Pointer to the exponent quaternion
 */
static inline void ebex_quat_pow(ebex_quat_t *m_dest, ebex_quat_t *m_base, ebex_quat_t *m_pow)
{
	ebex_quat_t tmp_quat;
	ebex_quat_log(&tmp_quat, m_pow);
	ebex_quat_mul(&tmp_quat, m_base, &tmp_quat);
	ebex_quat_exp(m_dest, &tmp_quat);
}

#endif /* EBEX_QUATERNION_H_ */
